//
// Created by lustre on 2023/1/15.
//

#include "Quadruped.h"

Adafruit_PWMServoDriver pwm;
TwoWire wire;

/**
 * 控制ticker每步具体动作，每个相邻的case 里内容经过ticker本身的延时分步执行。
 *
 * */
int step = -1;

void Quadruped::init(int sda, int scl) {
    wire.pins(sda, scl);

    pwm = Adafruit_PWMServoDriver(PCA9685_I2C_ADDRESS, wire);
    pwm.begin();
    pwm.setPWMFreq(PWM_FREQUENCY);
    for (Joint *joint: joints) {
        pwm.setPWM(joint->pin, 0, (joint->getRealLower() + joint->getRealUpper()) / 2);
    }
    Serial.println(rearLeftKnee.realLowerLimit);
}



/**
 * 每一步动作需要舵机转到对应的角度后才能进行下一个动作否则
 * 会导致直接忽略中间的动作，因而需要在同一个舵机两个转动之间delay <br>
 * 而由Ticker执行时不能使用delay函数，但Ticker每次执行是有延时的，将Ticker的间隔设为舵机转动需要的延时
 * step变量指向每步的动作
 *
 * */
// 左腿右迈左蹬 右腿左迈右蹬
void Quadruped::forward() {

    step = ++step % 6;
    switch (step) {
        case 0:
            frontLeftKnee.up();
            rightRearKnee.up();
            frontLeftLeg.right();
            rightRearLeg.left();
            return;
        case 1:
            frontLeftKnee.down();
            rightRearKnee.down();
            return;
        case 2:
            // 一边提腿一边蹬另两只腿 可以使蹬的腿承受部分压力增大摩擦力
            rightFrontKnee.up();
            rearLeftKnee.up();
            frontLeftLeg.left();
            rightRearLeg.right();
            return;
        case 3:
            rightFrontLeg.left();
            rearLeftLeg.right();
            return;
        case 4:
            rearLeftKnee.down();
            rightFrontKnee.down();
            return;
        case 5:
            frontLeftKnee.up();
            rightRearKnee.up();
            rightFrontLeg.right();
            rearLeftLeg.left();
            return;
    }


}

// 右腿右迈左蹬 左腿左迈右蹬
void Quadruped::backward() {
    step = ++step % 6;
    switch (step) {
        case 0:
            frontLeftKnee.up();
            rightRearKnee.up();
            frontLeftLeg.left();
            rightRearLeg.right();
            return;
        case 1:
            frontLeftKnee.down();
            rightRearKnee.down();
            return;
        case 2:
            rightFrontKnee.up();
            rearLeftKnee.up();
            frontLeftLeg.right();
            rightRearLeg.left();
            return;
        case 3:
            rightFrontLeg.right();
            rearLeftLeg.left();
            return;
        case 4:
            rearLeftKnee.down();
            rightFrontKnee.down();
            return;
        case 5:
            frontLeftKnee.up();
            rightRearKnee.up();
            rightFrontLeg.left();
            rearLeftLeg.right();
            return;
    }


}

// 左腿右迈左蹬 右腿右迈左蹬
void Quadruped::turnRight() {
    step = ++step % 6;
    switch (step) {
        case 0:
            frontLeftKnee.up();
            rightRearKnee.up();
            frontLeftLeg.right();
            rightRearLeg.right();
            return;
        case 1:
            frontLeftKnee.down();
            rightRearKnee.down();
            return;
        case 2:
            rightFrontKnee.up();
            rearLeftKnee.up();
            frontLeftLeg.left();
            rightRearLeg.left();
            return;
        case 3:
            rightFrontLeg.right();
            rearLeftLeg.right();
            return;
        case 4:
            rearLeftKnee.down();
            rightFrontKnee.down();
            return;
        case 5:
            frontLeftKnee.up();
            rightRearKnee.up();
            rightFrontLeg.left();
            rearLeftLeg.left();
            return;
    }
}

// 左腿左迈右蹬 右腿左迈右蹬
void Quadruped::turnLeft() {
    step = ++step % 6;
    switch (step) {
        case 0:
            frontLeftKnee.up();
            rightRearKnee.up();
            frontLeftLeg.left();
            rightRearLeg.left();
            return;
        case 1:
            frontLeftKnee.down();
            rightRearKnee.down();
            return;
        case 2:
            rightFrontKnee.up();
            rearLeftKnee.up();
            frontLeftLeg.right();
            rightRearLeg.right();
            return;
        case 3:
            rightFrontLeg.left();
            rearLeftLeg.left();
            return;
        case 4:
            rearLeftKnee.down();
            rightFrontKnee.down();
            return;
        case 5:
            frontLeftKnee.up();
            rightRearKnee.up();
            rightFrontLeg.right();
            rearLeftLeg.right();
            return;
    }
}

void Quadruped::stop() {
    setQuadrupedAction(-1);
}

void Quadruped::reset() {
    stop();
    for (Joint *joint: joints) {
        pwm.setPWM(joint->pin, 0, (joint->getRealLower() + joint->getRealUpper()) / 2);
    }
}

void Quadruped::turn(uint8_t target, uint16_t pos) {
    pwm.setPWM(target, 0, pos);
}

void Quadruped::doAsyncAction(int action) {
    void (Quadruped::*anAction)() = actionTab[action];
    (this->*anAction)();
}


Quadruped::Quadruped(const HipJoint &frontLeftLeg, const KneeJoint &frontLeftKnee,
                     const HipJoint &rightFrontLeg, const KneeJoint &rightFrontKnee,
                     const HipJoint &rightRearLeg, const KneeJoint &rightRearKnee,
                     const HipJoint &rearLeftLeg, const KneeJoint &rearLeftKnee) : frontLeftLeg(frontLeftLeg),
                                                                                   frontLeftKnee(frontLeftKnee),
                                                                                   rightFrontLeg(rightFrontLeg),
                                                                                   rightFrontKnee(rightFrontKnee),
                                                                                   rightRearLeg(rightRearLeg),
                                                                                   rightRearKnee(rightRearKnee),
                                                                                   rearLeftLeg(rearLeftLeg),
                                                                                   rearLeftKnee(rearLeftKnee) {}

volatile int Quadruped::getQuadrupedAction() const {
#ifdef ACTION_DEBUG
    Serial.printf("get action %d\n", quadrupedAction);
#endif

    return quadrupedAction;
}

void Quadruped::setQuadrupedAction(volatile int action) {
#ifdef ACTION_DEBUG
    Serial.printf("set action %d", action);
#endif
    // 将下一步重置为0
    step = -1;
    Quadruped::quadrupedAction = action;
}


KneeJoint::KneeJoint(int pin, int upperLimit, int lowerLimit) :
        Joint(pin, upperLimit, lowerLimit) {
    this->realLowerLimit = (END_POS - BASE_POS) * lowerLimit / 100 + BASE_POS;
    this->realUpperLimit = (END_POS - BASE_POS) * upperLimit / 100 + BASE_POS;
}

void KneeJoint::up() {
    pwm.setPWM(this->pin, 0, this->realUpperLimit);
}

void KneeJoint::down() {
    pwm.setPWM(this->pin, 0, this->realLowerLimit);
}


Joint::Joint(int pin, int upperLimit, int lowerLimit) : pin(pin), upperLimit(upperLimit), lowerLimit(lowerLimit) {}


HipJoint::HipJoint(int pin, int leftLimit, int rightLimit) : Joint(pin, leftLimit, rightLimit) {
    // 将指定的x 在 0 ~ 100的数字映射到 舵机转动范围 base ~ end
    // y = k * x + a
    // a = base 舵机起点
    // k = range * x / 100
    // range = end - base
    // y = (end -base) * x / 100 + base
    this->realLeftLimit = (END_POS - BASE_POS) * leftLimit / 100 + BASE_POS;
    this->realRightLimit = (END_POS - BASE_POS) * rightLimit / 100 + BASE_POS;

    // 现range 将缩减至 80% 则左右各缩减 10%
    // y1 = k1 * x + a1
    // base1 = base + range * 0.1
    // a1 = base1
    // end1 = base1 + range * 0.8
    // range1 = range * 0.8
    // k1 = range1 * x /100
    // y1 = 0.8 * (end -base) * x + base + (end -base) * 0.1
    // 用第一步的结果应该也可以，但先算除法会带来更多的精度丢失
    this->leftWalkingLimit = 8 * (END_POS - BASE_POS) * leftLimit / 1000 + BASE_POS + (END_POS - BASE_POS) / 10;
    this->rightWalkingLimit = 8 * (END_POS - BASE_POS) * rightLimit / 1000 + BASE_POS + (END_POS - BASE_POS) / 10;
}

void HipJoint::left() {
    pwm.setPWM(this->pin, 0, this->leftWalkingLimit);
}

void HipJoint::right() {
    pwm.setPWM(this->pin, 0, this->rightWalkingLimit);
}
